import sensor, image, time
import tv
from pyb import Pin
import time
from pyb import UART
#pin口测试，没有用
def PinTest(strPinName):
    pin = Pin(strPinName)
    pin.init(Pin.OUT)
    for retry in range(100):
        pin.low()
        if pin.value() != 0:
            return False
        pin.high()
        if pin.value() != 1:
            return False
    return True


red_threshold  = (0, 100, 49, 127, -12, 42)#红色阈值1，主要就调这个参数
#black_threshold = (1, 22, -17, -1, -3, 9)
#red_threshold  = (0, 100, 58, 127, -128, 127)#红色阈值2，主要就调这个参数
#red_threshold  = (0, 100, 53, 127, -20, 46)#红色阈值3，主要就调这个参数

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
sensor.set_brightness(1)#曝光度设为1，其实多少都无所谓，只是已经选了这个值就不要再改了，否则阈值还要调
clock = time.clock() # Tracks FPS.

tv.init() # Initialize the tv.
tv.channel(8) # For wireless video transmitter shield

#串口初始化
uart = UART(3, 57600, timeout_char=1000)
uart.init(57600, bits=8, parity=None, stop=1)

#四个方向PIN口输出初始化，未用

right = Pin('P0', Pin.OUT)
right.value(0)
left = Pin('P1', Pin.OUT)
left.value(0)
upper = Pin('P2', Pin.OUT)
upper.value(0)
down = Pin('P3', Pin.OUT)
down.value(0)


er = 0#偏差变量
#串口通信，发送坐标信息
def uart_transport(x_er,y_er):
    if x_er>2:
        er = x_er
        print("右偏")
        print(er)
        #data_temp = [0xFF,int(er),0x01]
        #data_t = bytearray(data_temp)
        #uart.write(data_t)
        uart.write(bytearray([0xFF]))
        uart.write(bytearray([int(er)]))
        uart.write(bytearray([0x01]))
    elif x_er<-2:
        er = -x_er
        print("左偏")
        print(er)
        #data_temp = [0xFF,int(er),0x00]
        #data_t = bytearray(data_temp)
        #uart.write(data_t)
        uart.write(bytearray([0xFF]))
        uart.write(bytearray([int(er)]))
        uart.write(bytearray([0x00]))
    elif x_er>=-2 and x_er<=2:
        print("x不偏")
        uart.write(bytearray([0xFF]))
        uart.write(bytearray([0x00]))
        uart.write(bytearray([0x00]))
        #y的都没用，只用X即可

    if y_er>5:
        er = y_er
        print("下偏")
        print(er)
        #data_temp = [0xFE,int(er),0x01]
        #data_t = bytearray(data_temp)
        #uart.write(data_t)
        uart.write(bytearray([0xFE]))
        uart.write(bytearray([int(er)]))
        uart.write(bytearray([0x01]))
    elif y_er<-5:
        er = -y_er
        print("上偏")
        print(er)
        #data_temp = [0xFE,int(er),0x00]
        #data_t = bytearray(data_temp)
        #uart.write(data_t)
        uart.write(bytearray([0xFE]))
        uart.write(bytearray([int(er)]))
        uart.write(bytearray([0x00]))
    elif y_er>=-5 and y_er<=5:
        print("y不偏")
        uart.write(bytearray([0xFE]))
        uart.write(bytearray([0x00]))
        uart.write(bytearray([0x00]))

#找最大色块函数
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

#主循环
while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_blobs([red_threshold])#检测色块
    if blobs:#检测到色块
        max_blob = find_max(blobs)#找到最大色块
        X_error = (max_blob.cx()-img.width()/2)-13.8#水平X方向偏差+补偿偏差,-为炮管向左补偿，+为炮管向右补偿
        Y_error = max_blob.cy()-img.height()/2#垂直Y方向偏差
        print("X_error: ", X_error)
        print("Y_error: ", Y_error)
        img.draw_rectangle(max_blob.rect()) # rect，画方框，框出最大色块
        img.draw_cross(max_blob.cx(), max_blob.cy()) # cx, cy，十字标记中心位置坐标
        uart_transport(X_error,Y_error)#串口发送偏差信息
        #电平输出偏差信息，未用
        if X_error>5:
            print("右偏")
            right.value(1)
            left.value(0)
        elif X_error<-5:
            print("左偏")
            left.value(1)
            right.value(0)
        elif X_error>=-5 and X_error<=5:
            print("X方向不偏")
            left.value(0)
            right.value(0)
        if Y_error>5:
            print("下偏")
            down.value(1)
            upper.value(0)
        elif Y_error<-5:
            print("上偏")
            upper.value(1)
            down.value(0)
        elif Y_error>=-5 and Y_error<=5:
            print("Y方向不偏")
            upper.value(0)
            down.value(0)

    else:#若为检测到红色，则输出全为0
        right.value(0)
        left.value(0)
        upper.value(0)
        down.value(0)
    tv.display(sensor.snapshot()) # Take a picture and display the image.
